#include "fd.h"
#include "asm.h"
#include "interrupts.h"


static u32 motor_on;
static volatile u32 interrupt_status;
static floppy_params_t floppy_disk;
static u8 st0, cylinder;

static void delay() {
  int i;

  for(i = 0; i < 1000000; ++i)
    ;
}

void fd_isr() {
  interrupt_status = 1;
}

void fd_init() {
  register_isr(FLOPPY_IRQ, fd_isr);
  int i;
  u8* ptr = &floppy_disk; 

  for(i = 0; i < sizeof(floppy_params_t); ++i)
    *(ptr + i) = *((u8*)(DISK_PARAMETER_ADDRESS + i));
  
  fd_motor_on(FD_PRIMARY_BASE, 0);
  delay();
}

void fd_motor_on(u32 base, u8 drive) {
  if(motor_on)
    return;

  switch(drive) {
    case 0:
      outb(base+FD_DOR, 0x14);
      break;
    case 1:
      outb(base+FD_DOR, 0x2C);
      break;
    case 2:
      outb(base+FD_DOR, 0x4C);
      break;
    case 3:
      outb(base+FD_DOR, 0x8C);
      break;
  }

  motor_on = 1;
  delay();
    
}

void fd_motor_off(u8 base) {
  outb(base+FD_DOR, 0);
}

void reset_floppy_controller(u32 base, u8 drive) { 
  interrupt_status = 0;
  outb((base + FD_DOR), 0x00); /*disable controller*/ 
  outb((base + FD_DOR), 0x14); /*enable controller*/ 
  motor_on = 1;  

  while(interrupt_status == 0)
    ;

  check_interrupt_status(base, &st0, &cylinder); 
  outb(base + FD_CCR, 0); 
  configure_drive(base, drive); 
  calibrate_drive(base, drive); 
  return; 
} 


//################################
void configure_drive(u32 base,u8 drive) {
  write_floppy_command(base, FIX_DRIVE_DATA); /*config/specify command*/ 
  write_floppy_command(base, floppy_disk.steprate_headunload); 
  write_floppy_command(base, floppy_disk.headload_ndma | 1); 
  return; 
} 

void calibrate_drive(u32 base, u8 drive) { 
  fd_motor_on(base, drive); 

  interrupt_status = 0;
  write_floppy_command(base, CALIBRATE_DRIVE); 
  write_floppy_command(base, drive); 
  
  /*wait for interrupt from controller*/ 
  while(interrupt_status == 0)
    ;

  /*check interrupt status and store results in global variables st0 and cylinder*/ 
  check_interrupt_status(base, &st0, &cylinder); 
  return; 
} 

void check_interrupt_status(u32 base, u8 *st0, u8 *cylinder) { 
  write_floppy_command(base,CHECK_INTERRUPT_STATUS); 
  wait_floppy_data(base); 
  *st0=inb(base + FD_DR); 
  wait_floppy_data(base); 
  *cylinder=inb(base + FD_DR); 
  return; 
} 

/* read main status register of controller and wait until it signals that data is ready in the data register*/ 
void wait_floppy_data(u32 base) { 
  while(((inb(base+FD_MSR)) & 0xd0)!= 0xd0)
    ; 

  return; 
} 

void write_floppy_command(u32 base,u8 command) { 
  wait_floppy_ready(base); 
  outb(base+FD_DR,command); 
  return; 
} 

void wait_floppy_ready(u32 base) {
  while((inb(base+FD_MSR) & 0x80) != 0x80)
    ;
}

void seek_track(u8 head, u8 cylinder, u8 drive) {
  write_floppy_command(FD_PRIMARY_BASE, SEEK_TRACK);

  // write:  0  0  0  0  0  head  ds1 ds0 (ds1 | ds0 = drive)
  write_floppy_command(FD_PRIMARY_BASE, 0);
  write_floppy_command(FD_PRIMARY_BASE, cylinder);
  interrupt_status = 0;

  while(interrupt_status == 0)
    ;

  check_interrupt_status(FD_PRIMARY_BASE,&st0,&cylinder); 
}

void read_sector(u8 sector, u8 head, u8 cylinder, u8 drive, u8* buffer) {
  seek_track(head,cylinder,drive); 


//  start_dma(0x02,0x44,buffer,511); 
//  k_delay(floppy_disk.head_settle_time); 



  write_floppy_command(FD_PRIMARY_BASE, READ_SECTOR); 
  write_floppy_command(FD_PRIMARY_BASE, head<<2|drive); 
  write_floppy_command(FD_PRIMARY_BASE, cylinder); 
  write_floppy_command(FD_PRIMARY_BASE, head); 
  write_floppy_command(FD_PRIMARY_BASE, sector); 
  write_floppy_command(FD_PRIMARY_BASE, floppy_disk.bytes_per_sector); /*sector size = 128*2^size*/ 
  write_floppy_command(FD_PRIMARY_BASE, floppy_disk.sectors_per_track); /*last sector*/ 
  write_floppy_command(FD_PRIMARY_BASE, floppy_disk.gap_length); /*27 default gap3 value*/ 
  write_floppy_command(FD_PRIMARY_BASE, floppy_disk.data_length); /*default value for data length*/ 
 
  interrupt_status = 0;
  int i;
  
  for(i = 0; i < 512; ++i) {
    while(interrupt_status == 0)
      ;

    *(buffer + i) = inb(FD_PRIMARY_BASE + FD_DR);
    interrupt_status = 0;
    check_interrupt_status(FD_PRIMARY_BASE,&st0,&cylinder);
  }

  check_interrupt_status(FD_PRIMARY_BASE,&st0,&cylinder); 
  read_result_phase(); 
  return; 
} 

void read_result_phase() { 
  u8 status; 
  u32 timeout=16; 

  while(timeout--) { 
    wait_floppy_ready(FD_PRIMARY_BASE); 
    status = inb(FD_PRIMARY_BASE+FD_MSR); 

    if((status&0xd0) != 0xd0) 
      break; 

    status=inb(FD_PRIMARY_BASE+FD_DR); 
  } 

  return; 
} 

